#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include "capture.h"

int capture_open(Capture *capture) {
    int fd = open(capture->dev, O_RDWR);
    if(fd < 0) return -1;
    capture->fd = fd;
    /* 检查兼容性 */
    struct v4l2_capability capability;
    int ret = ioctl(capture->fd, VIDIOC_QUERYCAP, &capability);
    if(ret < 0) {
        perror("VIDIOC_QUERYCAP");
        goto ERROR;
    }
    if(!(capability.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf(stderr, "NO CAP FOR VIDEO CAPTURE");
        goto ERROR;
    }
    if(!(capability.capabilities & V4L2_CAP_STREAMING)) {
        fprintf(stderr, "NO CAP FOR STREAMING");
        goto ERROR;
    }
    /* 枚举支持的图像格式 */
    int i = 0;
    char *fmtname = NULL;
    printf("ENUM FMT:\n");
    while(1) {
        struct v4l2_fmtdesc fmtdesc;
        fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmtdesc.index = i;
        int ret = ioctl(capture->fd, VIDIOC_ENUM_FMT, &fmtdesc);
        if(ret < 0) break;
        printf("%d: %s %d\n", i + 1, fmtdesc.description, fmtdesc.pixelformat);
        if(fmtdesc.pixelformat == capture->pixelformat) {
            fmtname = malloc(sizeof(fmtdesc.description));
            strcpy((char*)fmtname, (char*)fmtdesc.description);
        }
        i++;
    }
    /* 未获取到任何支持的格式 */
    if(i == 0) {
        fprintf(stderr, "NO VALID FMT\n");
        goto ERROR;
    }
    /* 未找到需要的格式 */
    if(!fmtname) {
        fprintf(stderr, "FMT NOT SUPPORTED\n");
        free(fmtname);
        goto ERROR;
    }
    printf("FMT FOUND: %s\n", fmtname);
    free(fmtname);
    struct v4l2_format fmt;
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    /* 检查支持的图像大小 */
    ret = ioctl(capture->fd, VIDIOC_G_FMT, &fmt);
    if(ret < 0) {
        perror("VIDIOC_G_FMT");
        goto ERROR;
    }
    printf("Max Width: %d Max Height: %d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
    if(capture->width > fmt.fmt.pix.width) {
        fprintf(stderr, "width too large: %d\n", capture->width);
        goto ERROR;
    }
    if(capture->height > fmt.fmt.pix.height) {
        fprintf(stderr, "height too large: %d\n", capture->height);
        goto ERROR;
    }
    /* 设置摄像头采集大小 */
    fmt.fmt.pix.width = capture->width;
    fmt.fmt.pix.height = capture->height;
    fmt.fmt.pix.pixelformat = capture->pixelformat;
    ret = ioctl(capture->fd, VIDIOC_S_FMT, &fmt);
    if(ret < 0) {
        perror("VIDIOC_S_FMT");
        goto ERROR;
    }
    printf("Set Width: %d Set Height: %d\n", capture->width, capture->height);
    capture->bytesperline = fmt.fmt.pix.bytesperline;
    capture->sizeimage = fmt.fmt.pix.sizeimage;
    /* 请求摄像头缓冲区 */
    struct v4l2_requestbuffers reqbuf;
    memset(&reqbuf, 0, sizeof(reqbuf));
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.memory = V4L2_MEMORY_MMAP;
    reqbuf.count = 4;
    ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
    if(ret < 0) {
        perror("VIDIOC_REQBUFS");
        goto ERROR;
    }
    /* 设置摄像头缓冲区内存映射 */
    capture->buffers = calloc(reqbuf.count, sizeof(*capture->buffers));
    for(i = 0; i < reqbuf.count; i++) {
        struct v4l2_buffer buffer;
        memset(&buffer, 0, sizeof(buffer));
        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;;
        buffer.memory = V4L2_MEMORY_MMAP;
        buffer.index = i;
        ret = ioctl(capture->fd, VIDIOC_QUERYBUF, &buffer);
        if(ret < 0) {
            perror("VIDIOC_QUERYBUF");
            free(capture->buffers);
            goto ERROR;
        }
        capture->buffers[i].length = buffer.length;
        capture->buffers[i].start = mmap(NULL, buffer.length, PROT_READ | PROT_WRITE,
                                         MAP_SHARED, capture->fd, buffer.m.offset);
    }
    capture->nbuffers = reqbuf.count;
    /* buffer放入缓冲区队列*/
    for(i = 0; i < capture->nbuffers; i++) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        ioctl(capture->fd, VIDIOC_QBUF, &buf);
    }
    return 0;

ERROR:
    close(fd);
    return -1;
}

int capture_start(Capture *capture) {
    enum v4l2_buf_type type;
    /* 打开摄像头 */
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(capture->fd, VIDIOC_STREAMON, &type);
    if(ret < 0) {
        perror("VIDIOC_STREAMON");
        return -1;
    }
    return 0;
}

int capture_stop(Capture *capture) {
    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(capture->fd, VIDIOC_STREAMOFF, &type);
    if(ret < 0) {
        perror("VIDIOC_STREAMOFF");
        return -1;
    }
    return 0;
}

int capture_close(Capture *capture) {
    int i;
    int ret = 0;
    for(i = 0; i < capture->nbuffers; i++) {
        int ret = munmap(capture->buffers[i].start, capture->buffers[i].length);
        if(ret < 0) {
            perror("munmap");
            ret = -1;
        }
    }
    close(capture->fd);
    capture->fd = -1;
    return ret;
}

char *capture_shot(Capture *capture) {
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    int ret = ioctl(capture->fd, VIDIOC_DQBUF, &buf);
    if(ret < 0) {
        perror("VIDIOC_DQBUF");
        return NULL;
    }
    char *pic = malloc(capture->sizeimage);
    memcpy(pic, capture->buffers[buf.index].start, capture->sizeimage);
    ret = ioctl(capture->fd, VIDIOC_QBUF, &buf);
    if(ret < 0) {
        perror("VIDIOC_QBUF");
        free(pic);
        return NULL;
    }
    return pic;
}
